#include <tf/transform_broadcaster.h>
#include <sensor_msgs/Imu.h>
#include <webots_ros/set_int.h>
#include <webots_ros/set_float.h>
#include <webots_ros/get_int.h>
#include <webots_ros/get_float.h>
#include <geometry_msgs/PointStamped.h>
#include "ros/ros.h"

#define TIME_STEP 32

using namespace std;
bool callbackCalled = false;
string model_name="my_robot";
static double GPSValues[3] = {0, 0, 0};
static double inertialUnitValues[4] = {0, 0, 0, 0};
/*******************************************************
* Function name ：broadcastTransform
* Description   ：tf函数
* Parameter     ：
* Return        ：无
**********************************************************/
void broadcastTransform()
{
  static tf::TransformBroadcaster br;
  tf::Transform transform;

  transform.setIdentity();
  br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"map","odom"));// 发送tf坐标关系

  transform.setOrigin(tf::Vector3(GPSValues[0],GPSValues[1],0));// 设置原点
  tf::Quaternion q(inertialUnitValues[0],inertialUnitValues[1],inertialUnitValues[2],inertialUnitValues[3]);// 四元数 ->欧拉角

  transform.setRotation(q); //设置旋转数据
  br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"odom","base_link"));// 发送tf坐标关系
  
  transform.setOrigin(tf::Vector3(0.31,0,0));// 设置原点
  tf::Quaternion q1(tf::Vector3(1,0,0),M_PI);
  transform.setRotation(q1);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", model_name+"/scan"));
}
/*******************************************************
* Function name ：gpsCallback
* Description   ：GPS回调函数，获取GPS消息
* Parameter     ：
        @ values   GPS位置消息值
* Return        ：无
**********************************************************/
void GPSCallback(const geometry_msgs::PointStamped::ConstPtr &values)
{
  GPSValues[0] = values->point.x;// x坐标
  GPSValues[1] = values->point.y;// y坐标
  GPSValues[2] = values->point.z;// z坐标
  callbackCalled = true;
  broadcastTransform(); // tf坐标转换
}
/*******************************************************
* Function name ：inertialUnitCallback
* Description   ：IMU回调函数，获取IMU消息
* Parameter     ：
        @ values   IMU位置消息值
* Return        ：无
**********************************************************/
void inertialUnitCallback(const sensor_msgs::Imu::ConstPtr &values) {
  inertialUnitValues[0] = values->orientation.x;
  inertialUnitValues[1] = values->orientation.y;
  inertialUnitValues[2] = values->orientation.z;
  inertialUnitValues[3] = values->orientation.w;
  // ROS_INFO("Inertial unit values (quaternions) are x=%f y=%f z=%f w=%f (time: %d:%d).", inertialUnitValues[0],
  //          inertialUnitValues[1], inertialUnitValues[2], inertialUnitValues[2], values->header.stamp.sec,
  //          values->header.stamp.nsec);
  callbackCalled = true;
  broadcastTransform();

}
int main(int argc, char **argv)
{
/////////////////////////////////////////GPS enable
    ros::init(argc, argv, "my_robot_tf", ros::init_options::AnonymousName);
    ros::NodeHandle n;
    ros::ServiceClient enable_gps_client;          
    webots_ros::set_int gps_Srv;
    ros::Subscriber sub_GPS_32; 

    enable_gps_client = n.serviceClient<webots_ros::set_int>(model_name+"/gps/enable"); // 使能GPS服务
    gps_Srv.request.value = TIME_STEP;

    // 判断gps使能服务是否成功
    if (enable_gps_client.call(gps_Srv) && gps_Srv.response.success) {
    ROS_INFO("gps enabled.");
    } else {
    if (!gps_Srv.response.success)
    ROS_ERROR("Failed to enable gps.");
    return 1;
    }
    sub_GPS_32.shutdown();
    enable_gps_client.shutdown();

    ros::ServiceClient set_range_finder_client;
    webots_ros::set_int range_finder_srv;
    ros::Subscriber sub_range_finder_color;

    set_range_finder_client = n.serviceClient<webots_ros::set_int>(model_name + "/scan/enable");
    range_finder_srv.request.value = TIME_STEP;

    if (set_range_finder_client.call(range_finder_srv) && range_finder_srv.response.success) {
    ROS_INFO("scan enabled.");
    } else {
    if (!range_finder_srv.response.success)
        ROS_ERROR("Sampling period is not valid.");
    ROS_ERROR("Failed to enable range-finder.");
    return 1;
    }
  /////////////////////////////////////////camera enable
    ros::ServiceClient enable_camera_client;
    webots_ros::set_int camera_srv;
    ros::Subscriber sub_camera_color;

    enable_camera_client = n.serviceClient<webots_ros::set_int>(model_name + "/kinect_color/enable");
    camera_srv.request.value = TIME_STEP;

    if (enable_camera_client.call(camera_srv) && camera_srv.response.success) {
      ROS_INFO("Camera enabled.");
    } else {
      if (camera_srv.response.success == -1)
        ROS_ERROR("Sampling period is not valid.");
      ROS_ERROR("Failed to enable camera.");
      return 1;
    }

    sub_camera_color.shutdown();
    enable_camera_client.shutdown();
    /////////////////////////////////////////IMU enable
    ros::ServiceClient enable_inertial_unit_client;          
    webots_ros::set_int inertial_unit_Srv;
    ros::Subscriber sub_inertial_unit_32; 

    enable_inertial_unit_client = n.serviceClient<webots_ros::set_int>(model_name+"/inertial_unit/enable"); //订阅IMU使能服务
    inertial_unit_Srv.request.value = TIME_STEP;
    // 判断是否使能成功
    if (enable_inertial_unit_client.call(inertial_unit_Srv) && inertial_unit_Srv.response.success) {
    ROS_INFO("inertial_unit enabled.");
    } 
    else {
    if (!inertial_unit_Srv.response.success)
    ROS_ERROR("Sampling period is not valid.");
    ROS_ERROR("Failed to enable inertial_unit.");
    return 1;
    }
    sub_inertial_unit_32.shutdown();
    enable_inertial_unit_client.shutdown();

    ros::Subscriber gps_sub=n.subscribe(model_name+"/gps/values", 1, GPSCallback);
    ros::Subscriber inertial_unit_sub = n.subscribe(model_name+"/inertial_unit/quaternion", 1, inertialUnitCallback);
    ros::spin();
    return 0;
}